#include "slam_ros/map_editor.h"
#include "slam/io/proto_stream.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/2d/probability_grid.h"
#include "slam/mapping/2d/submap_2d.h"
#include "slam_ros/ros_map.h"
#include "slam/transform/rigid_transform.h"
#include "slam/transform/transform.h"
#include "slam/io/internal/mapping_state_serialization.h"
#include "slam_ros/node.h"
#include "tf2_ros/transform_listener.h"
#include "slam_ros/utilities.h"
#include "match/line2Dup.h"
#include <memory>
#include <iostream>
#include <assert.h>
#include <chrono>

using namespace std;
using namespace cv;

namespace slam_ros{

static std::string match_file_path = std::string(getenv("HOME")) + "/match/";
static std::string map_path = std::string(getenv("HOME")) + "/map/";

double ToEulerAngles(Eigen::Quaterniond& q) {

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
    double roll = std::atan2(sinr_cosp, cosr_cosp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
    double yaw = std::atan2(siny_cosp, cosy_cosp);

    //temp
    return 0.0;
}

MapEditor::MapEditor(const rclcpp::NodeOptions& options) 
        : Node("map_editor", options) {
    if (!dirExists(match_file_path)) {
        LOG(ERROR) << "match_file_path directory can not be found: " << match_file_path;
        LOG(INFO) << "Create match_file_path directory: " << mkPath(match_file_path, 0777);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    // Create service
    edit_map_service_ = create_service<agv_srvs::srv::EditMap>(
        slam_ros::kEditMap,
        std::bind(&MapEditor::handleEditMap, this, std::placeholders::_1, std::placeholders::_2));

    // Create publisher
    edit_map_progress_pub_ = create_publisher<slam_ros_msgs::msg::MapdataProgress>(
        "edit_map_progress", rclcpp::QoS(1));

    // Create timer
    progress_timer_ = create_wall_timer(
        10ms, std::bind(&MapEditor::publishEditMapProgress, this));
}

bool MapEditor::loadMapYaml(const std::string& file_name) {
    try {
        YAML::Node loaded = YAML::LoadFile(file_name);
        if (!loaded) {
            RCLCPP_INFO(this->get_logger(), "Read YAML Failed. data is not yaml format...");
            return false;
        }

        double resolution = loaded["resolution"].as<double>();
        double origin_x = loaded["origin"].as<vector<double>>().at(0);
        double origin_y = loaded["origin"].as<vector<double>>().at(1);
        unsigned int cells_size_x = loaded["width"].as<int>();
        unsigned int cells_size_y = loaded["height"].as<int>();

        RCLCPP_INFO(this->get_logger(), "cells_size_x: %d cells_size_y: %d resolution: %f [origin_x, origin_y]: %f %f", 
                   cells_size_x, cells_size_y, resolution, origin_x, origin_y);
    }
    catch (const YAML::Exception& e) {
        RCLCPP_INFO(this->get_logger(), "Read %s error is %s", file_name.c_str(), e.what());
        return false;
    }
    return true;
}

void MapEditor::publishEditMapProgress() {
    absl::MutexLock lock(&mutex_);
    if(edit_map_progress_pub_->get_subscription_count() > 0) {
        edit_map_progress_pub_->publish(edit_map_progress_msg_);
    }
}

bool MapEditor::loadRtYaml(const std::string& file_name) {
    try {
        YAML::Node loaded = YAML::LoadFile(file_name);
        if (!loaded) {
            RCLCPP_INFO(this->get_logger(), "Read RTYAML Failed. data is not yaml format...");
            return false;
        }
        double origin_tx = loaded["translation"].as<vector<double>>().at(0);
        double origin_ty = loaded["translation"].as<vector<double>>().at(1);
        double origin_tz = loaded["translation"].as<vector<double>>().at(2);

        double origin_rw = loaded["rotation"].as<vector<double>>().at(0);
        double origin_rx = loaded["rotation"].as<vector<double>>().at(1);
        double origin_ry = loaded["rotation"].as<vector<double>>().at(2);
        double origin_rz = loaded["rotation"].as<vector<double>>().at(3);

        const Eigen::Vector3d transtation(origin_tx, origin_ty, origin_tz);
        const Eigen::Quaterniond rotation(origin_rw, origin_rx, origin_ry, origin_rz);

        map_init_pose_ = ::slam::transform::Rigid3d(transtation, rotation);
        map_init_quaternion_ = map_init_pose_.rotation();
        map_init_translation_ = map_init_pose_.translation();
        //map_init_yaw = ToEulerAngles(map_init_quaterniond);
    }
    catch (const YAML::Exception& e) {
        RCLCPP_INFO(this->get_logger(), "Read %s error is %s", file_name.c_str(), e.what());
        return false;
    }
    return true;
}

void MapEditor::writeOut(const std::string& pbstream_filename, const std::string& map_filestem,
                         const double resolution, double trans_x, double trans_y, double rot_w,
                         double rot_x, double rot_y, double rot_z) {
    ::slam::io::ProtoStreamReader reader(pbstream_filename);
    ::slam::io::ProtoStreamDeserializer deserializer(&reader);

    RCLCPP_INFO(this->get_logger(), "Loading submap slices from serialized data.");
    std::map<::slam::mapping::SubmapId, ::slam::io::SubmapSlice> submap_slices;
    ::slam::mapping::ValueConversionTables conversion_tables;
    ::slam::io::DeserializeAndFillSubmapSlices(&deserializer, &submap_slices, &conversion_tables);
    CHECK(reader.eof());

    RCLCPP_INFO(this->get_logger(), "Generating combined map image from submap slices.");
    auto result = ::slam::io::PaintSubmapSlices(submap_slices, resolution);

    string img_file_path = map_filestem + "_rotated";
    ::slam::io::StreamFileWriter pgm_writer(img_file_path + ".pgm");

    ::slam::io::Image image(std::move(result.surface));
    ::slam_ros::WritePgm(image, resolution, &pgm_writer);

    image.WriteJpg(img_file_path + ".jpg");
    image.WritePng(map_filestem + ".png");

    const Eigen::Vector2d origin(
                -result.origin.x() * resolution,
                (result.origin.y() - image.height()) * resolution);

    ::slam_ros::WriteYaml(resolution, origin, map_filestem + ".yaml", map_filestem + ".png");
    ::slam::io::StreamFileWriter rt_yaml_writer(map_filestem + "_rt.yaml");
    const std::string output = absl::StrCat(
                "translation: [", trans_x, ", ", trans_y,", 0.0]","\n"
                "rotation: [", rot_w, ", ", rot_x, ", ", rot_y, ", ", rot_z,"]");
    rt_yaml_writer.Write(output.data(), output.size());
}

void MapEditor::convertToImgWithoutLocation(const std::string& pbstream_filename, const std::string& map_filestem,
                                           const double resolution) {
    ::slam::io::ProtoStreamReader reader(pbstream_filename);
    ::slam::io::ProtoStreamDeserializer deserializer(&reader);

    RCLCPP_INFO(this->get_logger(), "Loading submap slices from serialized data.");
    std::map<::slam::mapping::SubmapId, ::slam::io::SubmapSlice> submap_slices;
    ::slam::mapping::ValueConversionTables conversion_tables;
    ::slam::io::DeserializeGlobalMapSlice(&deserializer, &submap_slices, &conversion_tables);
    CHECK(reader.eof());

    RCLCPP_INFO(this->get_logger(), "Generating combined map image from submap slices.");
    auto result = ::slam::io::PaintSubmapSlices(submap_slices, resolution);

    ::slam::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
    ::slam::io::Image image(std::move(result.surface));
    ::slam_ros::WritePgm(image, resolution, &pgm_writer);

    image.WriteJpg(map_filestem + ".jpg");
    image.WritePng(map_filestem + ".png");
}

/***********************************************************************/
/*********************Erasing  after Rotaring the Map*******************/
/***********************************************************************/

//工具函数：检查CPU对SIMD的支持情况
void MIPP_test(){
    std::cout << "MIPP tests" << std::endl;
    std::cout << "----------" << std::endl << std::endl;

    std::cout << "Instr. type:       " << mipp::InstructionType                  << std::endl;
    std::cout << "Instr. full type:  " << mipp::InstructionFullType              << std::endl;
    std::cout << "Instr. version:    " << mipp::InstructionVersion               << std::endl;
    std::cout << "Instr. size:       " << mipp::RegisterSizeBit       << " bits" << std::endl;
    std::cout << "Instr. lanes:      " << mipp::Lanes                            << std::endl;
    std::cout << "64-bit support:    " << (mipp::Support64Bit    ? "yes" : "no") << std::endl;
    std::cout << "Byte/word support: " << (mipp::SupportByteWord ? "yes" : "no") << std::endl;

#ifndef has_max_int8_t
    std::cout << "in this SIMD, int8 max is not inplemented by MIPP" << std::endl;
#endif

#ifndef has_shuff_int8_t
    std::cout << "in this SIMD, int8 shuff is not inplemented by MIPP" << std::endl;
#endif

    std::cout << "----------" << std::endl << std::endl;
}

class Timer
{
public:
    Timer() : beg_(clock_::now()) {}
    void reset() { beg_ = clock_::now(); }
    double elapsed() const {
        return std::chrono::duration_cast<second_>
                (clock_::now() - beg_).count(); }
    void out(std::string message = ""){
        double t = elapsed();
        std::cout << message << "\nelasped time:" << t << "s" << std::endl;
        reset();
    }
private:
    typedef std::chrono::high_resolution_clock clock_;
    typedef std::chrono::duration<double, std::ratio<1> > second_;
    std::chrono::time_point<clock_> beg_;
};

cv::Point getPointAffinedPos(const cv::Point &src, const cv::Point center, double angle)
{
    cv::Point dst;
    int x = src.x - center.x;
    int y = src.y - center.y;
    dst.x = cvRound(x * cos(angle) + y * sin(angle) + center.x);
    dst.y = cvRound(-x * sin(angle) + y * cos(angle) + center.y);
    return dst;
}

void EraseOriginalMap(string mode = "test", string map_id = "", int rect_leftop_x = 0, int rect_leftop_y = 0,
                      int erase_rect_width = 0, int erase_rect_height = 0, int width = 0, int height = 0){
    line2Dup::Detector detector(256, {4, 8});
    int width_template = width;
    int height_template = height;
    if(mode != "test"){
        Mat img = imread(map_path + map_id + ".pgm", IMREAD_GRAYSCALE);
        assert(!img.empty() && "check your img path");

        Rect roi(0, 0, width_template, height_template);
        img = img(roi).clone();
        Mat mask = Mat(img.size(), CV_8UC1, {255});

        // padding to avoid rotating out
        int padding = 100;
        cv::Mat padded_img = cv::Mat(img.rows + 2*padding, img.cols + 2*padding, img.type(), cv::Scalar::all(0));
        img.copyTo(padded_img(Rect(padding, padding, img.cols, img.rows)));

        cv::Mat padded_mask = cv::Mat(mask.rows + 2*padding, mask.cols + 2*padding, mask.type(), cv::Scalar::all(0));
        mask.copyTo(padded_mask(Rect(padding, padding, img.cols, img.rows)));

        shape_based_matching::shapeInfo_producer shapes(padded_img, padded_mask);
        shapes.angle_range = {0, 360};
        shapes.angle_step = 0.5;

        shapes.scale_range = {1}; // support just one
        shapes.produce_infos();
        std::vector<shape_based_matching::shapeInfo_producer::Info> infos_have_templ;
        string class_id = "test";

        bool is_first = true;
        // for other scales you want to re-extract points:
        // set shapes.scale_range then produce_infos; set is_first = false;

        int first_id = 0;
        float first_angle = 0;
        for(auto& info: shapes.infos){
            Mat to_show = shapes.src_of(info);

            std::cout << "\ninfo.angle: " << info.angle << std::endl;
            int templ_id;

            if(is_first){
                templ_id = detector.addTemplate(shapes.src_of(info), class_id, shapes.mask_of(info));
                first_id = templ_id;
                first_angle = info.angle;
                is_first = false;
            }else{
                templ_id = detector.addTemplate_rotate(class_id, first_id,
                                                       info.angle-first_angle,
                {shapes.src.cols/2.0f, shapes.src.rows/2.0f});
            }

            auto templ = detector.getTemplates("test", templ_id);
            for(int i=0; i<templ[0].features.size(); i++){
                auto feat = templ[0].features[i];
                cv::circle(to_show, {feat.x+templ[0].tl_x, feat.y+templ[0].tl_y}, 3, {0, 0, 255}, -1);
            }

            // will be faster if not showing this
            //imshow("train", to_show);
            //waitKey(1);

            std::cout << "templ_id: " << templ_id << std::endl;
            if(templ_id != -1){
                infos_have_templ.push_back(info);
            }
        }
        detector.writeClasses(match_file_path+"%s_templ.yaml");
        shapes.save_infos(infos_have_templ, match_file_path + "test_info.yaml");
        std::cout << "train end" << std::endl << std::endl;
    }else if(mode=="test"){

        //读取待擦除图像，此图像仅仅进行了旋转操作
        Mat only_rotated = imread(map_path + map_id + "_rotated.pgm", 0);
        if(only_rotated.empty()){
            LOG(INFO) << map_path + map_id + "_rotated.pgm" << " not exists";
            return;
        }
        int width_only_rotated = only_rotated.cols;
        int height_only_rotated = only_rotated.rows;
        //确定要擦除的矩形区域的左上角坐标，以及长宽
        Point rect_left_top_point(rect_leftop_x, rect_leftop_y);
        int rect_width = erase_rect_width;
        int rect_height = erase_rect_height;
        //定义要擦除的矩形区域Rect对象，此区域下面简称为“擦除矩形”
        Rect rect_erase(rect_left_top_point.x, rect_left_top_point.y, rect_width, rect_height);
        //定义擦除矩形的中心点
        Point center_erase(rect_left_top_point.x + rect_width / 2, rect_left_top_point.y + rect_height / 2);
        //将待擦除图像粘贴到长宽为二倍的背景图像中
        Mat container(height_only_rotated * 2, width_only_rotated * 2, CV_8UC1, cv::Scalar(128));//创建一个单通道矩阵，每个像素都是255
        Mat imageROI;
        imageROI = container(Rect(width_only_rotated / 2, height_only_rotated / 2, width_only_rotated, height_only_rotated));
        only_rotated.copyTo(imageROI, only_rotated);


        /*****在长宽增加为二倍的待测图像中执行模板匹配*****/
        std::vector<std::string> ids;
        ids.push_back("test");
        detector.readClasses(ids, match_file_path+"%s_templ.yaml");
        // angle & scale are saved here, fetched by match id
        auto infos = shape_based_matching::shapeInfo_producer::load_infos(match_file_path + "test_info.yaml");

        Mat test_img = container;
        assert(!test_img.empty() && "check your img path");

        int stride = 16;
        int n = test_img.rows/stride;
        int m = test_img.cols/stride;
        Rect roi(0, 0, stride*m , stride*n);
        Mat img = test_img(roi).clone();
        assert(img.isContinuous());

        std::cout << "test img size: " << img.rows * img.cols << std::endl << std::endl;

        Timer timer;
        auto matches = detector.match(img, 60, ids);
        timer.out();

        if(img.channels() == 1) cvtColor(img, img, COLOR_GRAY2BGR);

        std::cout << "matches.size(): " << matches.size() << std::endl;
        size_t top5 = 1;
        if(top5>matches.size()) top5=matches.size();
        for(size_t i=0; i<top5; i++){
            auto match = matches[i];
            auto templ = detector.getTemplates("test", match.template_id);
            float r_scaled = width_template /2.0f*infos[match.template_id].scale;
            // scaling won't affect this, because it has been determined by warpAffine
            // cv::warpAffine(src, dst, rot_mat, src.size()); last param
            float train_img_half_width = width_template /2.0f + 100;
            float train_img_half_height = height_template /2.0f + 100;

            // center x,y of train_img in test img
            float x =  match.x - templ[0].tl_x + train_img_half_width;
            float y =  match.y - templ[0].tl_y + train_img_half_height;

            //得到匹配后的包围模板图像的目标旋转矩形，长宽等于模板长宽
            cv::RotatedRect rotatedRectangle({x, y}, Size(width_template, height_template), -infos[match.template_id].angle);
            //取出目标旋转矩形的四点坐标
            cv::Point2f vertices[4];
            rotatedRectangle.points(vertices);
            cv::Point points[1][4];
            for (int i = 0; i < 4; ++i) {
                points[0][i] = vertices[i];
            }
            //定义指向点坐标信息的二维指针
            const cv::Point* ppt[1] = { points[0] };
            //围成目标旋转矩形的轮廓点的个数
            int npt[] = { 4 };
            //定义掩码图像的基本信息，含尺寸和通道数
            cv::Mat mask = cv::Mat::zeros(img.size(), CV_8UC1);
            //根据连接点坐标、点个数、轮廓个数等绘制掩码图像
            cv::fillPoly(mask, ppt, npt, 1, cv::Scalar(255));
            //使用掩码图像截取目标旋转矩形区域
            cv::Mat croppedImage;
            img.copyTo(croppedImage, mask);
            //在截取后的图像上画出擦除矩形
            rectangle(croppedImage, rect_erase, Scalar(200, 50, 50), 1);
            //imshow("with rect_erased rectangle", croppedImage);
            imwrite(map_path + "matched.png", croppedImage);
            //输出匹配结果信息
            std::cout << "\nmatch.template_id: " << match.template_id << std::endl;
            std::cout << "match.similarity: " << match.similarity << std::endl;


            /*****将擦除矩形的坐标映射回原始地图，在原始地图上执行擦除*****/

            int h = croppedImage.rows;
            int w = croppedImage.cols;
            int cX = w / 2;
            int cY = h / 2;
            cv::Point points_res[1][4];
            //计算擦除矩形的中心映射回原始地图的坐标值
            cv::Point rect_center_rotated;
            rect_center_rotated = getPointAffinedPos(center_erase, Point2f(cX, cY), -infos[match.template_id].angle * CV_PI / 180);
            //计算擦除矩形的4顶点映射回原始地图的坐标值
            for (int m = 0; m < 4; m++)
            {
                points_res[0][m] = getPointAffinedPos(points[0][m], Point2f(cX, cY), -infos[match.template_id].angle * CV_PI / 180);
            }
            //找出目标矩形4顶点映射回原始地图后，哪个是左上角
            int point_index = 0;
            int x_anchor = points_res[0][0].x;
            int y_anchor = points_res[0][0].y;
            for (int i = 0; i < 4; i++)
            {
                int x_temp = points_res[0][i].x;
                int y_temp = points_res[0][i].y;
                if (x_anchor - x_temp > width_template / 2)
                {
                    point_index = i;
                    x_anchor = x_temp;
                }
                if (y_anchor - y_temp > height_template / 2)
                {
                    point_index = i;
                    y_anchor = y_temp;
                }
            }
            //计算擦除矩形映射后的中心，与目标矩形映射后的左上角之间的坐标差值，此插值体现在原始地图上，就是擦除矩形的中心坐标
            float x_diff = rect_center_rotated.x - points_res[0][point_index].x;
            float y_diff = rect_center_rotated.y - points_res[0][point_index].y;
            //读取原始地图
            Mat original_map = imread(map_path + map_id + ".pgm",IMREAD_GRAYSCALE);
            //准备矩形填充所需要的各种参数，如坐标值，顶点个数，轮廓个数等
            cv::RotatedRect rotatedRect_mapping_back({ x_diff, y_diff }, Size(rect_width, rect_height), infos[match.template_id].angle);
            cv::Point2f vertices_mapping_back[4];
            rotatedRect_mapping_back.points(vertices_mapping_back);
            Point pts_mapping_back[1][4];
            for(int n = 0; n < 4; n++)
            {
                pts_mapping_back[0][n] = vertices_mapping_back[n];
            }
            const Point* ppt_mapping_back[2] = { pts_mapping_back[0]};
            int npt_mapping_back[] = { 4 };
            //根据擦除矩形在映射后的坐标围成的轮廓，在原地图进行填充，也就是擦除
            cv::fillPoly(original_map, ppt_mapping_back, npt_mapping_back, 1, Scalar(255,255,255));
            //imshow("original_map", original_map);
            imwrite(map_path + map_id + "_erased.pgm", original_map);
        }

        //imwrite(prefix+ "case1/res.png", img);
        //waitKey(0);
        std::cout << "test end" << std::endl << std::endl;
    }
}

//执行旋转和擦除功能的主函数
bool MapEditor::handleEditMap(const std::shared_ptr<agv_srvs::srv::EditMap::Request> request,
                              std::shared_ptr<agv_srvs::srv::EditMap::Response> response) {
    RCLCPP_INFO(this->get_logger(), "HandleEditMap called");
    edit_map_progress_msg_.current_mapfile_name = request->mapid;
    edit_map_progress_msg_.status = true;
    edit_map_progress_msg_.current_progress = 10;
    edit_map_progress_msg_.error = " ";
    string map_id = request->mapid;
    string src_map_name = map_path + map_id + ".pbstream";   //输入文件名，原地图
    string rotated_map_name = map_path + map_id + "_rotated.pbstream";   //输出文件名，被选转后的地图
    string rtyaml_file_name = map_path + map_id + "_rt.yaml";
    string yaml_file_name = map_path + map_id + ".yaml";
    if(map_id.empty())
    {
        response->status.message = "Map Id is null.";
        response->status.code = 1;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "Map Id is null.";
        return true;
    }
    if (access(src_map_name.c_str(), 0) == -1)
    {
        response->status.message = "The pbstream file does not exist.";
        response->status.code = 2;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "The pbstream file does not exist.";
        return true;
    }
    if (!loadRtYaml(rtyaml_file_name)) {
        response->status.message = "The rt yaml file does not exist.";
        response->status.code = 3;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "The rt yaml file does not exist.";
        return true;
    }

    //解析输入信息
    std::vector<agv_msgs::msg::MapEditPoly> apex_poly = request->erase_action.apex_poly;
    int erase_operation_type = apex_poly[0].attribute;   //0：测试，实际运行   1：训练，通过旋转至各角度得到模板值
    int erase_leftop_x = apex_poly[0].apex_points[0].x;
    int erase_leftop_y = apex_poly[0].apex_points[0].y;
    int erase_width = apex_poly[0].apex_points[3].x - apex_poly[0].apex_points[0].x;
    int erase_height = apex_poly[0].apex_points[3].y - apex_poly[0].apex_points[0].y;
    bool operate_erase = request->erase_action.operate;
    double angle = request->transform_action.angle;
    bool operate_transform = request->transform_action.operate;
    double x_transform = request->transform_action.x;
    double y_transform = request->transform_action.y;

    if(operate_transform){
        double Angle = angle;   //用 2*pai 减去传入的角度, 以实现客户端要求的从相反的方向旋转
        double translation_x = x_transform + request->transform_action.translation_x;
        double translation_y = y_transform + request->transform_action.translation_y;
        double quan_w = cos(Angle / 2);
        double quan_z = sin(Angle / 2);
        using Vector = Eigen::Matrix<double, 3, 1>;
        using Quaternion = Eigen::Quaternion<double>;
        using AngleAxis = Eigen::AngleAxis<double>;
        const Vector& translation_modify = { translation_x,translation_y, 0 };
        const Quaternion& rotation_num = { quan_w, 0, 0, quan_z };
        const Quaternion& rotation_modify = rotation_num.normalized();

        const Vector& translation_modify_init = { -x_transform,-y_transform, 0 };
        const Quaternion& rotation_num_init = { 1.0, 0.0, 0.0, 0.0 };
        ::slam::transform::Rigid3d pose_init = ::slam::transform::Rigid3d(translation_modify_init, rotation_num_init);
        //转换为Rigid3d
        const ::slam::transform::Rigid3d pose_to_impose = ::slam::transform::Rigid3d(translation_modify, rotation_modify) * pose_init;

        LOG(INFO) << "rotate_transform_map : start load  " << src_map_name.c_str();

        ::slam::io::ProtoStreamReader reader(src_map_name);
        ::slam::io::ProtoStreamDeserializer map_deserializer(&reader);
        ::slam::mapping::proto::PoseGraph map_graph = map_deserializer.pose_graph();

        ::slam::mapping::proto::SerializedData proto_;
        ::slam::io::ProtoStreamReader reader_(src_map_name);
        ::slam::mapping::proto::SerializationHeader header;
        reader_.ReadProto(&header);
        reader_.ReadProto(&proto_);

        const ::slam::transform::Rigid3d change_pose = pose_to_impose * map_init_pose_;

        //计数器
        int submap_index = 0;

        LOG(INFO) << "rotate_transform_map : start 修改全局节点坐标  ";
        edit_map_progress_msg_.current_progress = 40;
        //修改全局节点坐标
        for (int trajectory_id = 0; trajectory_id < proto_.pose_graph().trajectory_size(); trajectory_id++) {

            edit_map_progress_msg_.current_progress = 50;
            for (int i = 0; i < proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->node_size(); i++) {
                //LOG(INFO) << "trajectory_id = " << trajectory_id << "  node_id is " << i;
                const ::slam::transform::Rigid3d global_node_pose = change_pose *
                        (::slam::transform::ToRigid3(
                             map_graph.trajectory(trajectory_id)
                             .node(i)
                             .pose()));
                auto global_pose = proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->mutable_node(i)->mutable_pose();
                global_pose->mutable_translation()->set_x(global_node_pose.translation().x());
                global_pose->mutable_translation()->set_y(global_node_pose.translation().y());
                global_pose->mutable_translation()->set_z(0);
                global_pose->mutable_rotation()->set_w(global_node_pose.rotation().w());
                global_pose->mutable_rotation()->set_z(global_node_pose.rotation().z());
                global_pose->mutable_rotation()->set_y(0);
                global_pose->mutable_rotation()->set_x(0);
            }

            //修改全局子图坐标
            for (int k = 0; k < proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->submap_size(); k++) {
                const ::slam::transform::Rigid3d global_submap_pose = change_pose * (
                            ::slam::transform::ToRigid3(
                                map_graph.trajectory(trajectory_id)
                                .submap(submap_index)
                                .pose()));
                auto local_pose = proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->mutable_submap(submap_index)->mutable_pose();
                LOG(INFO) << "original submap_x = " << local_pose->mutable_translation()->x();
                LOG(INFO) << "original submap_y = " << local_pose->mutable_translation()->y();
                local_pose->mutable_translation()->set_x(global_submap_pose.translation().x());
                LOG(INFO) << "submap_x = " << local_pose->mutable_translation()->x();
                local_pose->mutable_translation()->set_y(global_submap_pose.translation().y());
                LOG(INFO) << "submap_y = " << local_pose->mutable_translation()->y();
                local_pose->mutable_translation()->set_z(0);
                local_pose->mutable_rotation()->set_w(global_submap_pose.rotation().w());
                local_pose->mutable_rotation()->set_z(global_submap_pose.rotation().z());
                local_pose->mutable_rotation()->set_y(0);
                local_pose->mutable_rotation()->set_x(0);
                submap_index++;
            }
            submap_index = 0;
        }
        edit_map_progress_msg_.current_progress = 60;

        //将修改后数据写出到新的pbstream文件
        LOG(INFO) << "rotate_transform_map : start write  " << rotated_map_name.c_str();
        ::slam::io::ProtoStreamWriter writer(rotated_map_name);
        writer.WriteProto(header);
        writer.WriteProto(proto_);
        while (reader_.ReadProto(&proto_))
        {
            writer.WriteProto(proto_);
        }
        writer.Close();

        //转换为pgm yaml并输出
        std::string rotated_imge_path = map_path + map_id;
        writeOut(rotated_map_name, rotated_imge_path, 0.05, change_pose.translation().x(), change_pose.translation().y(),
                 change_pose.rotation().w(), change_pose.rotation().x(),
                 change_pose.rotation().y(), change_pose.rotation().z());
        remove(rotated_map_name.c_str());
    }

    edit_map_progress_msg_.current_progress = 80;
    //调用旋转后擦除的功能
    if(operate_erase == 3){
        Mat src_map = imread(map_path + map_id + ".pgm", IMREAD_GRAYSCALE);
        if(src_map.empty()){
            LOG(ERROR) << map_id << ".pgm" << " not exists.";
        }else{
            int src_width = src_map.cols;
            int src_height = src_map.rows;
            string mode = "";
            0 == erase_operation_type ? mode = "test" : mode = "train";
            EraseOriginalMap(mode, map_id, erase_leftop_x, erase_leftop_y, erase_width, erase_height, src_width, src_height); // test or train
        }

    }
    //功能：从map目录下的map.pbstream导出pgm png 以及jpg文件
    //目的：用于从pgm导出为pbstream后，再转化为可视文件，方便观察导出结果
    //不使用时可关闭
    //  string pb_name = map_path + "map.pbstream";
    //  std::string converted_prefix = map_path + "map";
    //  if (access(pb_name.c_str(), 0) == -1)
    //  {
    //    LOG(INFO) << "The map.pbstream file does not exist.";
    //  }else{
    //    ConvertToImgWithoutLocation(pb_name, converted_prefix, 0.05);
    //  }

    //compress map
    // 待压缩文件列表
    const std::string map_pbs = map_path + map_id + ".pbstream";
    const std::string map_zip = map_path + map_id + ".tar.gz";
    const std::string map_png = map_path + map_id + ".png";
    const std::string map_yaml = map_path + map_id + ".yaml";
    const std::string map_rtyaml = map_path + map_id + "_rt.yaml";
    const std::string map_pgm = map_path + map_id + ".pgm";
    const std::string map_jpg = map_path + map_id + ".jpg";
    const std::string map_param = map_path + "config/";
    const std::string cmd = "tar -zcvf "  +
            map_zip  + " " +
            map_pbs + " " +
            map_png  + " " +
            map_yaml + " " +
            map_rtyaml + " " +
            map_param;

    int ret = system(cmd.c_str());

    if(ret == -1){
        response->status.message = "compress file faild.";
        response->status.code = 4;
        LOG(ERROR)<<"cannot compress map.";
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "compress map faild.";
        return true;
    }

    const std::string chmod_cmd = "echo 'vensin'| sudo -S chmod -R 777 "  + map_path;

    int chmod_ret = system(chmod_cmd.c_str());

    if(chmod_ret == -1 || chmod_ret == 256){
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.error = "permission is wrong.";
        LOG(ERROR)<<"cur map permission is wrong.";
        return true;
    }

    edit_map_progress_msg_.status = true;
    edit_map_progress_msg_.current_progress = 100;
    edit_map_progress_msg_.error = " ";
    //写反馈信息
    response->status.message = "All actions finished.";
    response->status.code = 0;
    return true;
}

bool MapEditor::AutoHandleEditMap(const std::shared_ptr<agv_srvs::srv::EditMap::Request> request,
        std::shared_ptr<agv_srvs::srv::EditMap::Response> response) {
    RCLCPP_INFO(this->get_logger(), "AutoHandleEditMap called");
    edit_map_progress_msg_.current_mapfile_name = request->mapid;
    edit_map_progress_msg_.status = true;
    edit_map_progress_msg_.current_progress = 10;
    edit_map_progress_msg_.error = " ";
    string map_id = request->mapid;
    string src_map_name = map_path + map_id + ".pbstream";   //输入文件名，原地图
    string rotated_map_name = map_path + map_id + "_rotated.pbstream";   //输出文件名，被选转后的地图
    string rtyaml_file_name = map_path + map_id + "_rt.yaml";
    string yaml_file_name = map_path + map_id + ".yaml";
    if(map_id.empty())
    {
        response->status.message = "Map Id is null.";
        response->status.code = 1;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "Map Id is null.";
        return true;
    }
    if (access(src_map_name.c_str(), 0) == -1)
    {
        response->status.message = "The pbstream file does not exist.";
        response->status.code = 2;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "The pbstream file does not exist.";
        return true;
    }
    if (!loadRtYaml(rtyaml_file_name)) {
        response->status.message = "The rt yaml file does not exist.";
        response->status.code = 3;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "The rt yaml file does not exist.";
        return true;
    }

    //解析输入信息
    std::vector<agv_msgs::msg::MapEditPoly> apex_poly = request->erase_action.apex_poly;
    int erase_operation_type = apex_poly[0].attribute;   //0：测试，实际运行   1：训练，通过旋转至各角度得到模板值
    int erase_leftop_x = apex_poly[0].apex_points[0].x;
    int erase_leftop_y = apex_poly[0].apex_points[0].y;
    int erase_width = apex_poly[0].apex_points[3].x - apex_poly[0].apex_points[0].x;
    int erase_height = apex_poly[0].apex_points[3].y - apex_poly[0].apex_points[0].y;
    bool operate_erase = request->erase_action.operate;
    double angle = request->transform_action.angle;
    bool operate_transform = request->transform_action.operate;
    double x_transform = request->transform_action.x;
    double y_transform = request->transform_action.y;

    if(operate_transform){
        double Angle = angle;   //用 2*pai 减去传入的角度, 以实现客户端要求的从相反的方向旋转
        double translation_x = x_transform + request->transform_action.translation_x;
        double translation_y = y_transform + request->transform_action.translation_y;
        double quan_w = cos(Angle / 2);
        double quan_z = sin(Angle / 2);
        using Vector = Eigen::Matrix<double, 3, 1>;
        using Quaternion = Eigen::Quaternion<double>;
        using AngleAxis = Eigen::AngleAxis<double>;
        const Vector& translation_modify = { translation_x,translation_y, 0 };
        const Quaternion& rotation_num = { quan_w, 0, 0, quan_z };
        const Quaternion& rotation_modify = rotation_num.normalized();

        const Vector& translation_modify_init = { -x_transform,-y_transform, 0 };
        const Quaternion& rotation_num_init = { 1.0, 0.0, 0.0, 0.0 };
        ::slam::transform::Rigid3d pose_init = ::slam::transform::Rigid3d(translation_modify_init, rotation_num_init);
        //转换为Rigid3d
        const ::slam::transform::Rigid3d pose_to_impose = ::slam::transform::Rigid3d(translation_modify, rotation_modify) * pose_init;

        LOG(INFO) << "rotate_transform_map : start load  " << src_map_name.c_str();

        ::slam::io::ProtoStreamReader reader(src_map_name);
        ::slam::io::ProtoStreamDeserializer map_deserializer(&reader);
        ::slam::mapping::proto::PoseGraph map_graph = map_deserializer.pose_graph();

        ::slam::mapping::proto::SerializedData proto_;
        ::slam::io::ProtoStreamReader reader_(src_map_name);
        ::slam::mapping::proto::SerializationHeader header;
        reader_.ReadProto(&header);
        reader_.ReadProto(&proto_);

        const ::slam::transform::Rigid3d change_pose = pose_to_impose * map_init_pose_;

        //计数器
        int submap_index = 0;

        LOG(INFO) << "rotate_transform_map : start 修改全局节点坐标  ";
        edit_map_progress_msg_.current_progress = 40;
        //修改全局节点坐标
        for (int trajectory_id = 0; trajectory_id < proto_.pose_graph().trajectory_size(); trajectory_id++) {

            edit_map_progress_msg_.current_progress = 50;
            for (int i = 0; i < proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->node_size(); i++) {
                //LOG(INFO) << "trajectory_id = " << trajectory_id << "  node_id is " << i;
                const ::slam::transform::Rigid3d global_node_pose = change_pose *
                        (::slam::transform::ToRigid3(
                             map_graph.trajectory(trajectory_id)
                             .node(i)
                             .pose()));
                auto global_pose = proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->mutable_node(i)->mutable_pose();
                global_pose->mutable_translation()->set_x(global_node_pose.translation().x());
                global_pose->mutable_translation()->set_y(global_node_pose.translation().y());
                global_pose->mutable_translation()->set_z(0);
                global_pose->mutable_rotation()->set_w(global_node_pose.rotation().w());
                global_pose->mutable_rotation()->set_z(global_node_pose.rotation().z());
                global_pose->mutable_rotation()->set_y(0);
                global_pose->mutable_rotation()->set_x(0);
            }

            //修改全局子图坐标
            for (int k = 0; k < proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->submap_size(); k++) {
                const ::slam::transform::Rigid3d global_submap_pose = change_pose * (
                            ::slam::transform::ToRigid3(
                                map_graph.trajectory(trajectory_id)
                                .submap(submap_index)
                                .pose()));
                auto local_pose = proto_.mutable_pose_graph()->mutable_trajectory(trajectory_id)->mutable_submap(submap_index)->mutable_pose();
                LOG(INFO) << "original submap_x = " << local_pose->mutable_translation()->x();
                LOG(INFO) << "original submap_y = " << local_pose->mutable_translation()->y();
                local_pose->mutable_translation()->set_x(global_submap_pose.translation().x());
                LOG(INFO) << "submap_x = " << local_pose->mutable_translation()->x();
                local_pose->mutable_translation()->set_y(global_submap_pose.translation().y());
                LOG(INFO) << "submap_y = " << local_pose->mutable_translation()->y();
                local_pose->mutable_translation()->set_z(0);
                local_pose->mutable_rotation()->set_w(global_submap_pose.rotation().w());
                local_pose->mutable_rotation()->set_z(global_submap_pose.rotation().z());
                local_pose->mutable_rotation()->set_y(0);
                local_pose->mutable_rotation()->set_x(0);
                submap_index++;
            }
            submap_index = 0;
        }
        edit_map_progress_msg_.current_progress = 60;

        //将修改后数据写出到新的pbstream文件
        LOG(INFO) << "rotate_transform_map : start write  " << rotated_map_name.c_str();
        ::slam::io::ProtoStreamWriter writer(rotated_map_name);
        writer.WriteProto(header);
        writer.WriteProto(proto_);
        while (reader_.ReadProto(&proto_))
        {
            writer.WriteProto(proto_);
        }
        writer.Close();

        //转换为pgm yaml并输出
        std::string rotated_imge_path = map_path + map_id;
        writeOut(rotated_map_name, rotated_imge_path, 0.05, change_pose.translation().x(), change_pose.translation().y(),
                 change_pose.rotation().w(), change_pose.rotation().x(),
                 change_pose.rotation().y(), change_pose.rotation().z());
        remove(rotated_map_name.c_str());
    }

    edit_map_progress_msg_.current_progress = 80;
    //调用旋转后擦除的功能
    if(operate_erase == 3){
        Mat src_map = imread(map_path + map_id + ".pgm", IMREAD_GRAYSCALE);
        if(src_map.empty()){
            LOG(ERROR) << map_id << ".pgm" << " not exists.";
        }else{
            int src_width = src_map.cols;
            int src_height = src_map.rows;
            string mode = "";
            0 == erase_operation_type ? mode = "test" : mode = "train";
            EraseOriginalMap(mode, map_id, erase_leftop_x, erase_leftop_y, erase_width, erase_height, src_width, src_height); // test or train
        }

    }
    //功能：从map目录下的map.pbstream导出pgm png 以及jpg文件
    //目的：用于从pgm导出为pbstream后，再转化为可视文件，方便观察导出结果
    //不使用时可关闭
    //  string pb_name = map_path + "map.pbstream";
    //  std::string converted_prefix = map_path + "map";
    //  if (access(pb_name.c_str(), 0) == -1)
    //  {
    //    LOG(INFO) << "The map.pbstream file does not exist.";
    //  }else{
    //    ConvertToImgWithoutLocation(pb_name, converted_prefix, 0.05);
    //  }

    //compress map
    // 待压缩文件列表
    const std::string map_pbs = map_path + map_id + ".pbstream";
    const std::string map_zip = map_path + map_id + ".tar.gz";
    const std::string map_png = map_path + map_id + ".png";
    const std::string map_yaml = map_path + map_id + ".yaml";
    const std::string map_rtyaml = map_path + map_id + "_rt.yaml";
    const std::string map_pgm = map_path + map_id + ".pgm";
    const std::string map_jpg = map_path + map_id + ".jpg";
    const std::string map_param = map_path + "config/";
    const std::string cmd = "tar -zcvf "  +
            map_zip  + " " +
            map_pbs + " " +
            map_png  + " " +
            map_yaml + " " +
            map_rtyaml + " " +
            map_param;

    int ret = system(cmd.c_str());

    if(ret == -1){
        response->status.message = "compress file faild.";
        response->status.code = 4;
        LOG(ERROR)<<"cannot compress map.";
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.error = "compress map faild.";
        return true;
    }

    const std::string chmod_cmd = "echo 'vensin'| sudo -S chmod -R 777 "  + map_path;

    int chmod_ret = system(chmod_cmd.c_str());

    if(chmod_ret == -1 || chmod_ret == 256){
        edit_map_progress_msg_.current_progress = 100;
        edit_map_progress_msg_.status = false;
        edit_map_progress_msg_.error = "permission is wrong.";
        LOG(ERROR)<<"cur map permission is wrong.";
        return true;
    }

    edit_map_progress_msg_.status = true;
    edit_map_progress_msg_.current_progress = 100;
    edit_map_progress_msg_.error = " ";
    //写反馈信息
    response->status.message = "All actions finished.";
    response->status.code = 0;
    return true;
}


}
